package modes;

import java.io.File;


import lejos.nxt.Sound;
import main.ColorSensorChew;
import main.Engine;
import main.SensorEngine;
import main.SonicSensor;
import navigation.Direction;

import navigation.IllegalMapPositionException;
import navigation.IllegalMapStateException;
import navigation.IllegalMoveException;
import navigation.Navigator;

/**
 *
 * @author Max
 */
public class ModeBlue extends AMode
{

    private int countWhite = 0;
    private final int MAX_COUNT_WHITE = 50;
    private int currentMaxWhite = MAX_COUNT_WHITE / 2;
    private int failureCount = 0;
    private int dir = -1;

    /**
     * Constructs a blue mode
     *
     * @param en Engine
     * @param se SensorEngine
     * @param cs ColorSensor
     * @param ss SonicSensor
     * @param navi Navigator
     */
    public ModeBlue(Engine en, SensorEngine se, ColorSensorChew cs, SonicSensor ss, Navigator navi)
    {
        super(en, se, cs, ss, navi);
    }

    /**
     * Chewie drives straightforward or turns left/right.
     * If the field is unkown, all field informatiom will be send to the navigator.
     * @return next mode
     * @throws InterruptedException
     */
    public Mode run()
            throws InterruptedException
    {
        Mode mode = Mode.BLUE;
        boolean first = false;
        engine.stop();
        rotationSensor.rotateMiddle();

        if (isReadMoreThanOnce())
        {
            Sound.playSample(new File("chewy1.wav"), 100);
            moveOneStep();
            if (isCrossNotFree())
            {
                return Mode.FOUND_OBJECT;
            }
            ModeController.dm.echo("Straight-Curve");
            turnBackToLine();

            if (navi.fieldIsKnown())
            {
                chooseDirection();
            } else
            {
                Sound.beep();
                first = true;
                unknownField();
            }
            mode = runTillBlack(mode);
            firstRead(first);

            if (isAfterCrossNotFree())
            {
                mode = Mode.FOUND_OBJECT;
            }
        } else
        {

            mode = Mode.BLACK;
        }
        resetBlue();
        dir = -1;
        return mode;
    }

    private void actionWhite()
            throws InterruptedException
    {
        if (rotationSensor.isMiddle())
        {
            rotationSensor.setRotating(false);
            if (countWhite == 0)
            {
                engine.switchRotation();
                failureCount++;
            } else if (countWhite >= currentMaxWhite)
            {
                failureMode();
            }
            countWhite++;
        } else if (rotationSensor.isLeft())
        {
            engine.driveRight(70);
        } else if (rotationSensor.isRight())
        {
            engine.driveLeft(70);
        }

    }

    private void actionBlue()
    {
        rotationSensor.setRotating(true);
        resetBlue();
        if (rotationSensor.isLeft())
        {
            engine.driveLeft(70);
        } else if (rotationSensor.isRight())
        {
            engine.driveRight(70);
        } else
        {
            engine.forward();
        }
    }

    private void firstRead(boolean first) throws InterruptedException
    {
        boolean left = false;
        boolean right = false;
        boolean forward = false;

        if (dir == 0)
        {
            ModeController.dm.echo("Drive-Right");
            right = true;
        } else if (dir == 2)
        {
            ModeController.dm.echo("Drive-Left");
            left = true;
        } else if (dir == 3)
        {
            forward = true;
            ModeController.dm.echo("Drive-Backward");
        } else
        {
            forward = true;
            ModeController.dm.echo("Drive-Forward");
        }
        if (first)
        {
            try
            {
                navi.addField(forward, right, true, left);
            } catch (IllegalMapPositionException ex)
            {
                ModeController.dm.echo(ex.toString());
            }
            ModeController.dm.clear();
            ModeController.dm.echo(navi.toString());
        }
        decidedDirection();

    }

    private void resetBlue()
    {
        countWhite = 0;
        currentMaxWhite = MAX_COUNT_WHITE / 2;
        failureCount = 0;
    }

    private void failureMode()
            throws InterruptedException
    {
        failureCount++;
        if (failureCount == 5)
        {
            dir = 2;
            turnToMiddle();
            engine.forward();
            Thread.sleep(700);
            engine.setSpeed(600);
            engine.rotateLeft();
        } else if (failureCount > 2 && failureCount < 5)
        {
            engine.switchRotation();
            countWhite = 0;
            currentMaxWhite += 25;
        } else if (failureCount == 2)
        {
            engine.switchRotation();
            countWhite = 0;
            currentMaxWhite = MAX_COUNT_WHITE;
        }

    }

    private boolean isReadMoreThanOnce()
            throws InterruptedException
    {
        boolean readMoreThanOnce = true;
        engine.stop();
        engine.rotateRight();

        for (int i = 0; i < MAX_COUNT_WHITE / 2; ++i)
        {
            color.readColor();
            if (!color.isColorBlue() && !color.isColorWhite())
            {
                engine.stop();
                readMoreThanOnce = false;
                break;
            }
        }
        if (readMoreThanOnce)
        {
            engine.rotateLeft();
            for (int i = 0; i < (MAX_COUNT_WHITE / 2) * 2; ++i)
            {
                color.readColor();
                if (!color.isColorBlue() && !color.isColorWhite())
                {
                    engine.stop();
                    readMoreThanOnce = false;
                    break;
                }
            }
        }
        return readMoreThanOnce;
    }

    private void unknownField()
            throws InterruptedException
    {
        engine.stop();
        findRightLine();
        rotationSensor.rotateMiddle();
        if (dir == -1)
        {

            engine.stop();
            dir = 1;
        }
    }

    private void chooseDirection()
            throws InterruptedException
    {
        engine.stop();
        try
        {
            if (navi.decideDirection() == Direction.RIGHT)
            {
                rotationSensor.rotate(90);
            } else if (navi.decideDirection() == Direction.LEFT)
            {
                rotationSensor.rotate(-90);
            }
        } catch (IllegalMapStateException ex)
        {
            ModeController.dm.echo(ex.toString());
            engine.stop();
            Sound.playSample(new File("chewy1.wav"), 100);
            Thread.sleep(500);
            Sound.playSample(new File("chewy1.wav"), 100);

        }
        engine.forward();
        for (int i = 0; i < 400; ++i)
        {
            color.readColor();
            if (!color.isColorWhite())
            {
                turn();
                break;
            }
        }
        rotationSensor.rotateMiddle();
        if (dir != -1)
        {
            return;
        }
        engine.stop();
        dir = 1;
    }

    private Mode runTillBlack(Mode mode)
            throws InterruptedException
    {
        do
        {
            rotationSensor.rotateColorSensor();
            color.readColor();
            if (color.isColorBlue())
            {
                actionBlue();
            } else if (color.isColorWhite())
            {
                actionWhite();
            } else if (color.isColorBlack())
            {
                actionBlue();
                mode = Mode.BLACK;
            }

        } while (mode == Mode.BLUE);
        return mode;
    }

    private Mode findNewWay(Mode mode)
            throws InterruptedException
    {
        rotationSensor.rotateMiddle();
        do
        {
            color.readColor();
            if (color.isColorBlue())
            {
                actionBlue();
            } else if (color.isColorBlack())
            {
                mode = Mode.BLACK;
                actionBlue();
            }

        } while ((!color.isColorBlue()) && (!color.isColorBlack()));
        return mode;
    }

    private void turnBackToLine()
    {
        engine.rotateRight();
        for (int k = 0; k < MAX_COUNT_WHITE / 2 + 10; ++k)
        {
            color.readColor();
            if (color.isColorBlue())
            {
            }
        }
        engine.stop();
    }

    private void moveOneStep()
    {
        try
        {
            navi.moveOneStep();
        } catch (IllegalMoveException ex)
        {
            ModeController.dm.echo(ex.toString());
        }
    }

    private void turn() throws InterruptedException
    {
        try
        {
            if (navi.decideDirection() == Direction.RIGHT)
            {
                engine.setSpeed(600);
                Thread.sleep(700);
                engine.rotateRight();
                dir = 0;
                findNewWay(Mode.BLUE);

            } else if (navi.decideDirection() == Direction.LEFT)
            {
                engine.setSpeed(600);
                Thread.sleep(700);
                engine.rotateLeft();
                dir = 2;
                findNewWay(Mode.BLUE);
            } else if (navi.decideDirection() == Direction.BACKWARD)
            {
                //engine.rotate180();
                dir = 3;

            }

        } catch (IllegalMapStateException ex)
        {
            ModeController.dm.echo(ex.toString());
            engine.stop();
            Sound.playSample(new File("chewy1.wav"), 100);
            Thread.sleep(500);
            Sound.playSample(new File("chewy1.wav"), 100);
        }
    }

    private void driveBackward() throws InterruptedException
    {
        if (dir == 0)
        {
            engine.rotateRight();
            Thread.sleep(200);
            findNewWay(Mode.BLUE);
        } else if (dir == 2)
        {
            engine.rotateLeft();
            Thread.sleep(200);
            findNewWay(Mode.BLUE);
        } else
        {
            engine.rotate180();
        }


    }

    private void findRightLine() throws InterruptedException
    {
        rotationSensor.rotate(90);
        engine.forward();
        for (int i = 0; i < 200; ++i)
        {
            color.readColor();
            if (!color.isColorWhite())
            {
                engine.setSpeed(600);
                Thread.sleep(700);
                engine.rotateRight();
                dir = 0;
                findNewWay(Mode.BLUE);
                break;
            }

        }
        engine.stop();
    }

    private void turnToMiddle()
    {
        engine.switchRotation();
        for (int k = 0; k < MAX_COUNT_WHITE / 2; ++k)
        {
            color.readColor();
            if (color.isColorBlue())
            {
            }
        }
        engine.stop();
    }

    private void decidedDirection() throws InterruptedException
    {
        int direct = -1;
        try
        {
            direct = navi.decideDirection();
        } catch (IllegalMapStateException ex)
        {
        }
        if (direct == Direction.RIGHT)
        {
            navi.directionChanged(Direction.RIGHT);
        } else if (direct == Direction.LEFT)
        {
            navi.directionChanged(Direction.LEFT);
        } else if (direct == Direction.BACKWARD)
        {
            driveBackward();
            navi.directionChanged(Direction.BACKWARD);
        } else
        {
            navi.directionChanged(Direction.FORWARD);
        }


    }
}
